<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Aria: armExample.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Aria
   &#160;<span id="projectnumber">2.9.1</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('armExample_8cpp-example.html','');});
</script>
<div id="doc-content">
<div class="header">
  <div class="headertitle">
<div class="title">armExample.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>Example program showing use of the original 5DOF P2 arm accessory.Demonstrates how to connect with the Pioneer 2 controller and set up the P2Arm class, including the ARMpac handler. It simply queries and prints the status of the arm, moving it to a position, then exits. Note, The P2Arm class is only for use with the microcontroller-integrated original 5DOF Pioneer 2 arm (Robotica arm). The newer Energid Cyton 7DOF arm is accessed through separate cyton libraries.</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &quot;Aria.h&quot;</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)</div><div class="line">{</div><div class="line">  <a name="a0"></a><a class="code" href="classAria.html#ad99c16b5d947229d9f8e1c5b2d4cdd73">Aria::init</a>();</div><div class="line">  <a name="_a1"></a><a class="code" href="classArSimpleConnector.html">ArSimpleConnector</a> con(&amp;argc, argv);</div><div class="line">  <a name="_a2"></a><a class="code" href="classArRobot.html">ArRobot</a> robot;</div><div class="line">  <a name="_a3"></a><a class="code" href="classArP2Arm.html">ArP2Arm</a> arm;</div><div class="line"></div><div class="line"></div><div class="line">  <span class="keywordflow">if</span>(!<a name="a4"></a><a class="code" href="classAria.html#a1b090c01d88bb420b8cf8e0384d25ee1">Aria::parseArgs</a>()) </div><div class="line">  {</div><div class="line">    <a name="a5"></a><a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">    <a name="a6"></a><a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">    <span class="keywordflow">return</span> 1;</div><div class="line">  }</div><div class="line"></div><div class="line">  <a name="a7"></a><a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a8"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;armExample: Connecting to the robot.&quot;</span>);</div><div class="line">  <span class="keywordflow">if</span>(!con.<a name="a9"></a><a class="code" href="classArSimpleConnector.html#aecf2650aabb8fa3c48aed82b947a4a81">connectRobot</a>(&amp;robot))</div><div class="line">  {</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a10"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;armExample: Could not connect to the robot. Exiting.&quot;</span>);</div><div class="line">    <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">    <span class="keywordflow">return</span> 1;</div><div class="line">  }</div><div class="line">  robot.<a name="a11"></a><a class="code" href="classArRobot.html#a3c3d878cfd11a2ef91230005a83bfcaa">runAsync</a>(<span class="keyword">true</span>);</div><div class="line"></div><div class="line">  <span class="comment">// turn off sonar</span></div><div class="line">  robot.<a name="a12"></a><a class="code" href="classArRobot.html#aceef80a6274fb34aafcc422e4b1778c1">comInt</a>(28, 0);</div><div class="line"></div><div class="line">  <span class="comment">// Set up and initialize the arm</span></div><div class="line">  arm.<a name="a13"></a><a class="code" href="classArP2Arm.html#ab8c55e24c8f53131dc2038fdee2e2dd2">setRobot</a>(&amp;robot);</div><div class="line">  <span class="keywordflow">if</span> (arm.<a name="a14"></a><a class="code" href="classArP2Arm.html#a4a27053618b7aaf5530a827ba43bcd65">init</a>() != <a name="a15"></a><a class="code" href="classArP2Arm.html#a5a4a41ffa5c5b09876d7c90ce608bd94a2ffcdb24a114a9398e330f1184366035">ArP2Arm::SUCCESS</a>)</div><div class="line">  {</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;armExample: Error initializing the P2 Arm!&quot;</span>);</div><div class="line">    <span class="keywordflow">return</span> 1;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="comment">// Print out some of the settings</span></div><div class="line">  <a name="_a16"></a><a class="code" href="classP2ArmJoint.html">P2ArmJoint</a> *joint;</div><div class="line">  printf(<span class="stringliteral">&quot;Current joint info:\nJoint   Vel  Home  Center\n&quot;</span>);</div><div class="line">  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=1; i&lt;=<a name="a17"></a><a class="code" href="classArP2Arm.html#a6afea03f0ce9cf6c8b13d3422183f37e">ArP2Arm::NumJoints</a>; i++)</div><div class="line">  {</div><div class="line">    joint = arm.<a name="a18"></a><a class="code" href="classArP2Arm.html#a7cb23bc28fc661565c386df8c57e43e0">getJoint</a>(i);</div><div class="line">    printf(<span class="stringliteral">&quot;  %2i:  %5i %5i   %5i\n&quot;</span>, i, joint-&gt;<a name="a19"></a>myVel, joint-&gt;<a name="a20"></a>myHome, joint-&gt;<a name="a21"></a>myCenter);</div><div class="line">  }</div><div class="line">  printf(<span class="stringliteral">&quot;\n&quot;</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Put the arm to work</span></div><div class="line">  printf(<span class="stringliteral">&quot;Powering on  (takes a couple seconds to stabilize)\n&quot;</span>);</div><div class="line">  arm.<a name="a22"></a><a class="code" href="classArP2Arm.html#a918cf76f54441d9404e06ad0a88e72e8">powerOn</a>();</div><div class="line"></div><div class="line">  <span class="comment">// Request one status packet and print out the arm&#39;s status</span></div><div class="line">  printf(<span class="stringliteral">&quot;Current arm status:\n&quot;</span>);</div><div class="line">  arm.<a name="a23"></a><a class="code" href="classArP2Arm.html#acbc67ecfa961741eecd892e121a644c1">requestStatus</a>(<a name="a24"></a><a class="code" href="classArP2Arm.html#a635bd05c565cd4e3b2ae8ca6358c22afa2a201348300a434a162245436e17b70c">ArP2Arm::StatusSingle</a>);</div><div class="line">  <a name="a25"></a><a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(200);  <span class="comment">// Give time to get the packet</span></div><div class="line">  printf(<span class="stringliteral">&quot;Arm Status: &quot;</span>);</div><div class="line">  <span class="keywordflow">if</span> (arm.<a name="a26"></a><a class="code" href="classArP2Arm.html#aaf319ff9a65a79235e49909448b36489">getStatus</a>() &amp; <a name="a27"></a><a class="code" href="classArP2Arm.html#ac48d3c00dcda6b0c346cf514a5f46a0f">ArP2Arm::ArmGood</a>)</div><div class="line">    printf(<span class="stringliteral">&quot;Good=1 &quot;</span>);</div><div class="line">  <span class="keywordflow">else</span></div><div class="line">    printf(<span class="stringliteral">&quot;Good=0 &quot;</span>);</div><div class="line">  <span class="keywordflow">if</span> (arm.<a class="code" href="classArP2Arm.html#aaf319ff9a65a79235e49909448b36489">getStatus</a>() &amp; <a name="a28"></a><a class="code" href="classArP2Arm.html#a8d72666aabcd21cebb7c7f132a24f94c">ArP2Arm::ArmInited</a>)</div><div class="line">    printf(<span class="stringliteral">&quot;Inited=1 &quot;</span>);</div><div class="line">  <span class="keywordflow">else</span></div><div class="line">    printf(<span class="stringliteral">&quot;Inited=0 &quot;</span>);</div><div class="line">  <span class="keywordflow">if</span> (arm.<a class="code" href="classArP2Arm.html#aaf319ff9a65a79235e49909448b36489">getStatus</a>() &amp; <a name="a29"></a><a class="code" href="classArP2Arm.html#ad78fa5c42e06c7f02b4ece45e14a15dc">ArP2Arm::ArmPower</a>)</div><div class="line">    printf(<span class="stringliteral">&quot;Power=1 &quot;</span>);</div><div class="line">  <span class="keywordflow">else</span></div><div class="line">    printf(<span class="stringliteral">&quot;Power=0 &quot;</span>);</div><div class="line">  <span class="keywordflow">if</span> (arm.<a class="code" href="classArP2Arm.html#aaf319ff9a65a79235e49909448b36489">getStatus</a>() &amp; <a name="a30"></a><a class="code" href="classArP2Arm.html#a1f7933080af12540cc5fc72275097e98">ArP2Arm::ArmHoming</a>)</div><div class="line">    printf(<span class="stringliteral">&quot;Homing=1 &quot;</span>);</div><div class="line">  <span class="keywordflow">else</span></div><div class="line">    printf(<span class="stringliteral">&quot;Homing=0 &quot;</span>);</div><div class="line">  printf(<span class="stringliteral">&quot;\n\n&quot;</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Move the arm joints to specific positions</span></div><div class="line">  printf(<span class="stringliteral">&quot;Moving Arm...\n&quot;</span>);</div><div class="line">  <span class="keywordtype">int</span> deploy_offset[] = {0, -100, 10, 40, 80, -55, 20};</div><div class="line">  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=1; i&lt;=<a class="code" href="classArP2Arm.html#a6afea03f0ce9cf6c8b13d3422183f37e">ArP2Arm::NumJoints</a>; i++)</div><div class="line">  {</div><div class="line">    joint = arm.<a class="code" href="classArP2Arm.html#a7cb23bc28fc661565c386df8c57e43e0">getJoint</a>(i);</div><div class="line">    arm.<a name="a31"></a><a class="code" href="classArP2Arm.html#af5bd00a6e1a45ed1828dcd850f56a9a0">moveToTicks</a>(i, joint-&gt;myCenter + deploy_offset[i]);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="comment">// Wait for arm to achieve new position, printing joint positions and M for</span></div><div class="line">  <span class="comment">// moving, NM for not moving.</span></div><div class="line">  arm.<a class="code" href="classArP2Arm.html#acbc67ecfa961741eecd892e121a644c1">requestStatus</a>(<a name="a32"></a><a class="code" href="classArP2Arm.html#a635bd05c565cd4e3b2ae8ca6358c22afa7006376e59275c2ec3e16c64c6588146">ArP2Arm::StatusContinuous</a>);</div><div class="line">  <a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(300); <span class="comment">// wait a moment for arm status packet update with joints moving</span></div><div class="line">  <span class="keywordtype">bool</span> moving = <span class="keyword">true</span>;</div><div class="line">  <span class="keywordflow">while</span> (moving)</div><div class="line">  {</div><div class="line">      moving = <span class="keyword">false</span>;</div><div class="line">      printf(<span class="stringliteral">&quot;Joints: &quot;</span>);</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=1; i&lt;=<a class="code" href="classArP2Arm.html#a6afea03f0ce9cf6c8b13d3422183f37e">ArP2Arm::NumJoints</a>; i++)</div><div class="line">      {</div><div class="line">        printf(<span class="stringliteral">&quot;[%d] %.0f, &quot;</span>, i, arm.<a name="a33"></a><a class="code" href="classArP2Arm.html#ae7611f41a8624861a573f1e2599de56c">getJointPos</a>(i));</div><div class="line">        <span class="keywordflow">if</span> (arm.<a name="a34"></a><a class="code" href="classArP2Arm.html#ae802764f1ea7c94bdd234b548ce39367">getMoving</a>(i))</div><div class="line">        {</div><div class="line">            printf(<span class="stringliteral">&quot;M;  &quot;</span>);</div><div class="line">            moving = <span class="keyword">true</span>;</div><div class="line">          }</div><div class="line">        <span class="keywordflow">else</span></div><div class="line">        {</div><div class="line">          printf(<span class="stringliteral">&quot;NM; &quot;</span>);</div><div class="line">        }</div><div class="line">        }</div><div class="line">      printf(<span class="stringliteral">&quot;\r&quot;</span>);</div><div class="line">  }</div><div class="line">  printf(<span class="stringliteral">&quot;\n\n&quot;</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Return arm to park, wait, and disconnect. (Though the arm will automatically park</span></div><div class="line">  <span class="comment">// on client disconnect)</span></div><div class="line">  printf(<span class="stringliteral">&quot;Parking arm.\n&quot;</span>);</div><div class="line">  arm.<a name="a35"></a><a class="code" href="classArP2Arm.html#a61c1b3d3f500d5beb8b90e37b8865dc8">park</a>();</div><div class="line"></div><div class="line">  <span class="comment">// Wait 5 seconds or until arm shuts off</span></div><div class="line">  <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 5; (i &gt; 0) &amp;&amp; (arm.<a class="code" href="classArP2Arm.html#aaf319ff9a65a79235e49909448b36489">getStatus</a>() &amp; <a class="code" href="classArP2Arm.html#ad78fa5c42e06c7f02b4ece45e14a15dc">ArP2Arm::ArmPower</a>); i--)</div><div class="line">  {</div><div class="line">    <a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(1000);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="comment">// Disconnect from robot, etc., and exit.</span></div><div class="line">  printf(<span class="stringliteral">&quot;Shutting down ARIA and exiting.\n&quot;</span>);</div><div class="line">  <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(0);</div><div class="line">  <span class="keywordflow">return</span>(0);</div><div class="line">}</div><div class="line"></div></div><!-- fragment --> </div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
